#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           LeftBaseBack,  tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, PIDControl, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, PIDControl, reversed, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port5,           LeftConveyor,  tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           RightConveyor, tmotorVex269, openLoop)
#pragma config(Motor,  port7,           flippermotor,  tmotorVex393, openLoop)
#pragma config(Motor,  port8,           turntablemotor, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           RightBaseFront, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_3, 1000)
#pragma config(Motor,  port10,          RightBaseBack, tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_2, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
// test
#define Move1 100
#define Move2 750
#define Move3 100
#define Move4 100
#define Move5 100
#define Move6 100
#define Move7 100
#define Move8 100
#define Move9 215
#define Move10 260
#define Move11 100
#define Move12 100
#define Move13 100
#define Move14 100
#define Move15 100
#define Move16 100
#define Move17 100
#define Move18 100
#define Move19 100
#define Move20 100
#define Move21 400
#define Move22 100
#define Move23 100
#define Move24 100
#define Move25 1000

#define SpeedSlow 20
#define SpeedModerate 66
#define SpeedFast 127

#define FlipperToFloor 0
#define FlipperScore -100
#define FlipperUp 100
#define FlipperHoldHeavy 25
#define FlipperHoldLight 15
#define FlipperFast 127
#define FlipperSlow 60

#define ArmsFloor 0
#define ArmsTrough 1000
#define ArmsHigh 2000
#define ArmsFast 127
#define ArmsSlow 60

int iRampSpeed = 1;

////////////////////////////////////////////////////
//			Programming Skills Functions
////////////////////////////////////////////////////

void Turn90Degrees(bool bTurnRight, int iSpeed)
{
		//will read gyro to determine 90 degrees
		// the 2 wheel motors, one will be postitive one will be negative depending on which way we turn
	int degrees10 = 900;
	if (bTurnRight)
	{
		while (abs(SensorValue[in8]) < degrees10)
		{
			motor[LeftBaseBack] = iSpeed;
			motor[LeftBaseFront] = iSpeed;
			motor[RightBaseBack] = -(iSpeed);
			motor[RightBaseFront] = -(iSpeed);
		}
	}
	else
	{
		while (abs(SensorValue[in8]) < degrees10)
		{
			motor[LeftBaseBack]= -(iSpeed);
			motor[LeftBaseFront] = -(iSpeed);
			motor[RightBaseBack] = iSpeed;
			motor[RightBaseFront] = iSpeed;
		}
	}
}

void MoveForwardStraight(int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed - 50;
		motor(LeftBaseFront) = iSpeed - 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed - 50;
		motor(RightBaseFront) = iSpeed - 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

void MoveBackwardStraight (int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed + 50;
		motor(LeftBaseFront) = iSpeed + 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed + 50;
		motor(RightBaseFront) = iSpeed + 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

int GetRampVal(int maxPwr, int distTotal, int rampDist, int encoder)
{
   int minPwr = 50; //this should be the minimum power level required for reliable forward motion
   //set motors to maxPwr
	 int pwr = maxPwr;
   if(encoder < distTotal - rampDist)
   {
     //go forward at full speed until it's time to ramp down
   }

   if(encoder < distTotal) //ramp down time
   {
      int pwr = maxPwr - (maxPwr - minPwr)*(encoder - distTotal + rampDist)/rampDist; //this scales the power from max->minPwr over the rampDist
      //set motors to new ramped pwr
      return (pwr);
   }
   return (pwr);
}

void RampUp(int iSpeed, int iDistance)
{
		// Ramp up as we start (the first 1/2 second - we should always move at least 1/2 second)
	   int pwr = 1;
	   for(int t = 1; t < iSpeed; t += 1)
	   {
	     // increment the power by t each time through the loop - starts at 50 and builds
	   	 pwr = pwr + t;
	     //set motors to pwr
	   	 int iLE = abs(nMotorEncoder(LeftBaseBack));
			 //int iRE = abs(nMotorEncoder(RightBaseBack));
	     //MoveForwardStraight(pwr, iLE, iRE);
			 motor(LeftBaseBack) = pwr;
			 motor(LeftBaseFront) = pwr;
			 motor(RightBaseBack) = pwr;
			 motor(RightBaseFront) = pwr;
	     if (iLE > iDistance)
	     {
					motor(LeftBaseBack) = 0;
					motor(LeftBaseFront) = 0;
					motor(RightBaseBack) = 0;
					motor(RightBaseFront) = 0;
					return;
	   	 }
	   	 if (pwr < 50)
	   	 {
	     	wait1Msec(100);
	     }
	   }
}
// bForward = true is forwards, false is backwards
//iDistance tells the encoder how far to go
void Move(bool bForward, int iDistance, int iSpeed)
{
	//Reset encoders
	nMotorEncoder(LeftBaseFront) = 0;
	nMotorEncoder(LeftBaseBack) = 0;
	nMotorEncoder(RightBaseFront) = 0;
	nMotorEncoder(RightBaseBack) = 0;
	// get the current value (will be zero to start cause we just reset it)
	int iLE = 1;// abs(nMotorEncoder(LeftBaseBack));
	//int iLE = abs(nMotorEncoder(LeftBaseFront));
	int iRE =1; // abs(nMotorEncoder(RightBaseBack));
	if (bForward)
	{
		RampUp(iSpeed, iDistance);
		// move forward until we reach encoder value given as iDistance
		while (iLE < iDistance)
		{
			//get the percentage we have travelled
			float iPCTDist = (float)iLE/(float)iDistance;
			if (iPCTDist > .50)
			{
				iRampSpeed = iSpeed - (iSpeed * iPCTDist);
				if (iRampSpeed < 50)
				{
					iRampSpeed = 50;
				}
			}
			else
			{
				iRampSpeed = iSpeed;
			}
			MoveForwardStraight(iRampSpeed, iLE, iRE);
			//int iRampPwr = GetRampVal(iSpeed, iDistance, (iDistance - 200), iLE);
			//MoveForwardStraight(iRampPwr, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	else
	{
		// we are moving backwards
		while (iLE < iDistance)
		{
			MoveBackwardStraight(-iSpeed, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	motor(LeftBaseBack) = 0;
	motor(LeftBaseFront) = 0;
	motor(RightBaseBack) = 0;
	motor(RightBaseFront) = 0;
}


// We may not need to worry about a flipper hold position - because we may create the flipper to have a "C" shape which would
// mean that we could scoop up items and move the flipper back far enough so that a hold value would not be necessary. Then we
// would just be moving the flipper between pickup-dump to trough-raise to store bags)
void FlipperHold(bool bHeavy)
{
	if (bHeavy)
	{
		motor[flippermotor] = FlipperHoldHeavy;
	}
	else
	{
		motor[flippermotor] = FlipperHoldLight;
	}
}

void Flipper(bool bUp, int iPosition, int iMotorPower)
{
	int iCurrentPosition = SensorValue[FlipperPot];
	if (bUp)
	{
		// while the current position is lower than where we want to be, keep raising the flipper higher
		while (iCurrentPosition < iPosition)
		{
			motor[flippermotor] = iMotorPower;
			iCurrentPosition = SensorValue[FlipperPot];
		}
	}
	else
	{
		// while the current position is higher than where we want to be, keep lowering the flipper
		while (iCurrentPosition > iPosition)
		{
			motor[flippermotor] = -(iMotorPower);
			iCurrentPosition = SensorValue[FlipperPot];
		}
	}

}

void Arms(bool bUp, int iPosition, int iMotorPower)
{
	//Move arms to trough, floor, or high

	// This is the same as the flipper code above - assuming we don't have to program for arm states...
	// if we have to program for arm states - then hold off here...

	// check the current position of the arm's potentiometer

	// are we moving up or down
	// etc...
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);

	//Completely clear out any previous sensor readings by setting the port to "sensorNone"
	SensorType[in8] = sensorNone;
	wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	SensorType[in8] = sensorGyro;
	wait1Msec(2000);

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	Move(true,2000, SpeedFast);
	wait1Msec(2000);
	Move(true,100, SpeedFast);
}



/////////////////////////////////////////////////////////////////////////////////////////
//                                 User Control Task
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	while (true)
	{
		// Programming Skills Oly - no user code in here
		UserControlCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
	}
}
